
#ifndef __MLS_COMPARISON_H__
#define __MLS_COMPARISON_H__

#include "mlsRaw.h"

#include <cpplapack.h>
typedef CPPL::dgematrix    LMatrix;   // a CPPLapack matrix
typedef CPPL::dcovector    LVector;   // a CPPLapack vector
typedef CPPL::drovector    LRVector;   // a CPPLapack vector

#include "typedefs.h"
#include "jacobi.h"
#include "svdcmp.h"

#define USELAPACK 0

/// Multiplies matrices A and B: C = A*B
void multiply(const Mat_IO& A, const Mat_IO& B, Mat_IO& C) {
   
   for (int i=0; i<A.nrows(); ++i)
      for (int j=0; j<B.ncols(); ++j) {
         C[i][j] = 0;
         for (int k=0; k<A.ncols(); ++k)
            C[i][j] += A[i][k]*B[k][j];
      }
}

/// A solution involving orthonormal matrices
void mlsEval_OM_Horn(const vector<Point3> &P, const vector<Point3> &Q, float p[], float pd[]) {
   
   /// compute weighted centroids
   float pstar[3], qstar[3];
   weightedCentroids(p, pstar, qstar, P, Q);
   
   /// compute the correlation matrix
   float M0[3], M1[3], M2[3]; // the matrix rows
   float dotA;
   correlationMatrix(p, pstar, qstar, P, Q, M0, M1, M2, dotA);
   
   // transpose
   float M[3][3], Mt[3][3], S[3][3];
   M[0][0]=M0[0]; M[0][1]=M0[1]; M[0][2]=M0[2];
   M[1][0]=M1[0]; M[1][1]=M1[1]; M[1][2]=M1[2];
   M[2][0]=M2[0]; M[2][1]=M2[1]; M[2][2]=M2[2];
   Mt[0][0]=M[0][0]; Mt[0][1]=M[1][0]; Mt[0][2]=M[2][0];
   Mt[1][0]=M[0][1]; Mt[1][1]=M[1][1]; Mt[1][2]=M[2][1];
   Mt[2][0]=M[0][2]; Mt[2][1]=M[1][2]; Mt[2][2]=M[2][2];
   multiply(Mt,M,S);
   
   #if USELAPACK
   vector<double> er, ei;
   vector<LVector> evr, evi;
   LMatrix SS(3,3);
   SS(0,0)=S[0][0]; SS(0,1)=S[0][1]; SS(0,2)=S[0][2];
   SS(1,0)=S[1][0]; SS(1,1)=S[1][1]; SS(1,2)=S[1][2];
   SS(2,0)=S[2][0]; SS(2,1)=S[2][1]; SS(2,2)=S[2][2];
   SS.dgeev(er,ei,evr,evi); // eigensystem
   
   // build the rotation matrix
   float Si[3][3];
   Si[0][0]=1.0/sqrt(er[0]); Si[0][1]=0.0;             Si[0][2]=0.0;
   Si[1][0]=0.0;             Si[1][1]=1.0/sqrt(er[1]); Si[1][2]=0.0;
   Si[2][0]=0.0;             Si[2][1]=0.0;             Si[2][2]=1.0/sqrt(er[2]);
   float A[3][3], At[3][3];
   A[0][0]=evr[0](0); A[0][1]=evr[0](1); A[0][2]=evr[0](2);
   A[1][0]=evr[1](0); A[1][1]=evr[1](1); A[1][2]=evr[1](2);
   A[2][0]=evr[2](0); A[2][1]=evr[2](1); A[2][2]=evr[2](2);
   At[0][0]=A[0][0]; At[0][1]=A[1][0]; At[0][2]=A[2][0];
   At[1][0]=A[0][1]; At[1][1]=A[1][1]; At[1][2]=A[2][1];
   At[2][0]=A[0][2]; At[2][1]=A[1][2]; At[2][2]=A[2][2];
   
   float R[3][3], tmp1[3][3], tmp2[3][3];
   multiply(M,At,tmp1);
   multiply(Si,A,tmp2);
   multiply(tmp1,tmp2,R); 
   float vhat[3] = {p[0]-pstar[0], p[1]-pstar[1], p[2]-pstar[2]};
   float vx = vhat[0]*R[0][0] + vhat[1]*R[0][1] + vhat[2]*R[0][2];
   float vy = vhat[0]*R[1][0] + vhat[1]*R[1][1] + vhat[2]*R[1][2];
   float vz = vhat[0]*R[2][0] + vhat[1]*R[2][1] + vhat[2]*R[2][2];
   pd[0] = qstar[0] + vx;
   pd[1] = qstar[1] + vy;
   pd[2] = qstar[2] + vz;

   #else
   Mat_IO SS(3,3); Vec_O er(3); Mat_O evr(3,3);
   SS[0][0]=S[0][0]; SS[0][1]=S[0][1]; SS[0][2]=S[0][2];
   SS[1][0]=S[1][0]; SS[1][1]=S[1][1]; SS[1][2]=S[1][2];
   SS[2][0]=S[2][0]; SS[2][1]=S[2][1]; SS[2][2]=S[2][2];
   int nrot;
   jacobi(SS, er, evr, nrot);
   
   // build the rotation matrix
   float Si[3][3];
   Si[0][0]=1.0/sqrt(er[0]); Si[0][1]=0.0;             Si[0][2]=0.0;
   Si[1][0]=0.0;             Si[1][1]=1.0/sqrt(er[1]); Si[1][2]=0.0;
   Si[2][0]=0.0;             Si[2][1]=0.0;             Si[2][2]=1.0/sqrt(er[2]);
   
   float A[3][3], At[3][3];
   A[0][0]=evr[0][0]; A[0][1]=evr[0][1]; A[0][2]=evr[0][2];
   A[1][0]=evr[1][0]; A[1][1]=evr[1][1]; A[1][2]=evr[1][2];
   A[2][0]=evr[2][0]; A[2][1]=evr[2][1]; A[2][2]=evr[2][2];
   At[0][0]=A[0][0]; At[0][1]=A[1][0]; At[0][2]=A[2][0];
   At[1][0]=A[0][1]; At[1][1]=A[1][1]; At[1][2]=A[2][1];
   At[2][0]=A[0][2]; At[2][1]=A[1][2]; At[2][2]=A[2][2];
   
   float R[3][3], tmp1[3][3], tmp2[3][3];
   multiply(M,A,tmp1);
   multiply(Si,At,tmp2);
   multiply(tmp1,tmp2,R); 
   float vhat[3] = {p[0]-pstar[0], p[1]-pstar[1], p[2]-pstar[2]};
   float vx = vhat[0]*R[0][0] + vhat[1]*R[0][1] + vhat[2]*R[0][2];
   float vy = vhat[0]*R[1][0] + vhat[1]*R[1][1] + vhat[2]*R[1][2];
   float vz = vhat[0]*R[2][0] + vhat[1]*R[2][1] + vhat[2]*R[2][2];
   pd[0] = qstar[0] + vx;
   pd[1] = qstar[1] + vy;
   pd[2] = qstar[2] + vz;
   #endif
}

/// A solution involving unitary quaternions
void mlsEval_Quat(const vector<Point3> &P, const vector<Point3> &Q, float p[], float pd[]) {
   
   /// compute weighted centroids
   float pstar[3], qstar[3];
   weightedCentroids(p, pstar, qstar, P, Q);
   
   /// compute the correlation matrix
   float M0[3], M1[3], M2[3]; // the matrix rows
   float dotA;
   correlationMatrix(p, pstar, qstar, P, Q, M0, M1, M2, dotA);
   
   #if USELAPACK
   // build a quaternion-based eigenvalue system
   LMatrix A(4,4);
   A(0,0)=M0[0]+M1[1]+M2[2]; A(0,1)=M2[1]-M1[2];       A(0,2)=M0[2]-M2[0];        A(0,3) = M1[0]-M0[1];
   A(1,0)=A(0,1);            A(1,1)=M0[0]-M1[1]-M2[2]; A(1,2)=M0[1]+M1[0];        A(1,3) = M2[0]+M0[2];
   A(2,0)=A(0,2);            A(2,1)=A(1,2);            A(2,2)=-M0[0]+M1[1]-M2[2]; A(2,3) = M1[2]+M2[1];
   A(3,0)=A(0,3);            A(3,1)=A(1,3);            A(3,2)=A(2,3);             A(3,3) =-M0[0]-M1[1]+M2[2];
   vector<double> er, ei;
   vector<LRVector> evr, evi;
   A.dgeev(er, ei, evr, evi);
   int l = 0; // index of the largest eingenvalue
   for (int i=0; i<4; ++i) if (er[l] < er[i]) l = i;

   const Number &q0 = evr[l](0);
   const Number &q1 = evr[l](1);
   const Number &q2 = evr[l](2);
   const Number &q3 = evr[l](3);

   float R[3][3];
   R[0][0]=q0*q0+q1*q1-q2*q2-q3*q3; R[0][1]=2.0*(q1*q2-q0*q3);       R[0][2]=2.0*(q1*q3+q0*q2);
   R[1][0]=2.0*(q2*q1+q0*q3);       R[1][1]=q0*q0-q1*q1+q2*q2-q3*q3; R[1][2]=2.0*(q2*q3-q0*q1);
   R[2][0]=2.0*(q3*q1-q0*q2);       R[2][1]=2.0*(q3*q2+q0*q1);       R[2][2]=q0*q0-q1*q1-q2*q2+q3*q3;
   
   float vhat[3] = {p[0]-pstar[0], p[1]-pstar[1], p[2]-pstar[2]};
   float vx = vhat[0]*R[0][0] + vhat[1]*R[0][1] + vhat[2]*R[0][2];
   float vy = vhat[0]*R[1][0] + vhat[1]*R[1][1] + vhat[2]*R[1][2];
   float vz = vhat[0]*R[2][0] + vhat[1]*R[2][1] + vhat[2]*R[2][2];
   pd[0] = qstar[0] + vx;
   pd[1] = qstar[1] + vy;
   pd[2] = qstar[2] + vz;

   #else
   
   Mat_IO A(4,4); Vec_O er(4); Mat_O evr(4,4);
   A[0][0]=M0[0]+M1[1]+M2[2]; A[0][1]=M2[1]-M1[2];       A[0][2]=M0[2]-M2[0];        A[0][3] = M1[0]-M0[1];
   A[1][0]=A[0][1];           A[1][1]=M0[0]-M1[1]-M2[2]; A[1][2]=M0[1]+M1[0];        A[1][3] = M2[0]+M0[2];
   A[2][0]=A[0][2];           A[2][1]=A[1][2];           A[2][2]=-M0[0]+M1[1]-M2[2]; A[2][3] = M1[2]+M2[1];
   A[3][0]=A[0][3];           A[3][1]=A[1][3];           A[3][2]=A[2][3];            A[3][3] =-M0[0]-M1[1]+M2[2];
   int nrot;
   jacobi(A, er, evr, nrot);
   //cout<<"# of rotations: "<<nrot<<endl;

   int l = 0; // index of the largest eingenvalue
   for (int i=0; i<4; ++i) if (er[l] < er[i]) l = i;
   
   const Number &q0 = evr[l][0];
   const Number &q1 = evr[l][1];
   const Number &q2 = evr[l][2];
   const Number &q3 = evr[l][3];
   
   float R[3][3];
   R[0][0]=q0*q0+q1*q1-q2*q2-q3*q3; R[0][1]=2.0*(q1*q2-q0*q3);       R[0][2]=2.0*(q1*q3+q0*q2);
   R[1][0]=2.0*(q2*q1+q0*q3);       R[1][1]=q0*q0-q1*q1+q2*q2-q3*q3; R[1][2]=2.0*(q2*q3-q0*q1);
   R[2][0]=2.0*(q3*q1-q0*q2);       R[2][1]=2.0*(q3*q2+q0*q1);       R[2][2]=q0*q0-q1*q1-q2*q2+q3*q3;

   float vhat[3] = {p[0]-pstar[0], p[1]-pstar[1], p[2]-pstar[2]};
   float vx = vhat[0]*R[0][0] + vhat[1]*R[1][0] + vhat[2]*R[2][0];
   float vy = vhat[0]*R[0][1] + vhat[1]*R[1][1] + vhat[2]*R[2][1];
   float vz = vhat[0]*R[0][2] + vhat[1]*R[1][2] + vhat[2]*R[2][2];
   pd[0] = qstar[0] + vx;
   pd[1] = qstar[1] + vy;
   pd[2] = qstar[2] + vz;
   
   #endif
}

/// A solution involving unitary quaternions
void mlsEval_SVD(const vector<Point3> &P, const vector<Point3> &Q, float p[], float pd[]) {
   
   /// compute weighted centroids
   float pstar[3], qstar[3];
   weightedCentroids(p, pstar, qstar, P, Q);
   
   /// compute the correlation matrix
   float M0[3], M1[3], M2[3]; // the matrix rows
   float dotA;
   correlationMatrix(p, pstar, qstar, P, Q, M0, M1, M2, dotA);
   
   #if USELAPACK
   LMatrix MQ(3,3);
   MQ(0,0)=M0[0]; MQ(0,1)=M0[1]; MQ(0,2)=M0[2];
   MQ(1,0)=M1[0]; MQ(1,1)=M1[1]; MQ(1,2)=M1[2];
   MQ(2,0)=M2[0]; MQ(2,1)=M2[1]; MQ(2,2)=M2[2];
   MQ = t(MQ);
   
   LMatrix U(3,3), Vt(3,3); LVector S(3);
   MQ.dgesvd(S, U, Vt); // singular value decomposition
   LMatrix R = U*Vt; // rotation matrix

   LMatrix vhat(1,3);
   vhat(0,0) = p[0]-pstar[0];
   vhat(0,1) = p[1]-pstar[1];
   vhat(0,2) = p[2]-pstar[2];
   const LMatrix mv = vhat*R;
   pd[0] = qstar[0] + mv(0,0);
   pd[1] = qstar[1] + mv(0,1);
   pd[2] = qstar[2] + mv(0,2);

   #else
   
   Mat_IO A(3,3); Vec_O w(3); Mat_IO V(3,3);
   A[0][0]=M0[0]; A[0][1]=M1[0]; A[0][2]=M2[0];
   A[1][0]=M0[1]; A[1][1]=M1[1]; A[1][2]=M2[1];
   A[2][0]=M0[2]; A[2][1]=M1[2]; A[2][2]=M2[2];
   
   int its;
   svdcmp(A,w,V,its); // singular value decomposition
   //cout<<"# of iterations: "<<its<<endl;
   Mat_IO Vt(3, 3);
   for (int i=0; i<3; ++i) 
      for (int j=0; j<3; ++j) 
         Vt[i][j] = V[j][i];
   
   Mat_IO R(3,3);
   multiply(A, Vt, R);

   float vhat[3] = {p[0]-pstar[0], p[1]-pstar[1], p[2]-pstar[2]};
   float vx = vhat[0]*R[0][0] + vhat[1]*R[1][0] + vhat[2]*R[2][0];
   float vy = vhat[0]*R[0][1] + vhat[1]*R[1][1] + vhat[2]*R[2][1];
   float vz = vhat[0]*R[0][2] + vhat[1]*R[1][2] + vhat[2]*R[2][2];
   pd[0] = qstar[0] + vx;
   pd[1] = qstar[1] + vy;
   pd[2] = qstar[2] + vz;
   #endif
}

/// implemenation of method by SoHyeon Jeong
void mlsEval_nearly(const vector<Point3> &P, const vector<Point3> &Q, float p[], float pd[]) {
   
   /// compute weighted centroids
   float pstar[3], qstar[3];
   weightedCentroids(p, pstar, qstar, P, Q);
   
   const unsigned int NCTRLPOINTS = P.size();
   vector<Vector3> vri(NCTRLPOINTS, Vector3(0,0,0));
   float pjhat[3], qjhat[3];
   for (unsigned int j=0; j<NCTRLPOINTS; j++) {
      pjhat[0] = P[j][0]-pstar[0];  pjhat[1] = P[j][1]-pstar[1]; pjhat[2] = P[j][2]-pstar[2];
      qjhat[0] = Q[j][0]-qstar[0];  qjhat[1] = Q[j][1]-qstar[1]; qjhat[2] = Q[j][2]-qstar[2];
      
      Vector3 pi(pjhat[0], pjhat[1], pjhat[2]);
      Vector3 qi(qjhat[0], qjhat[1], qjhat[2]);
      Number dot = pi*qi;
      if (dot > 1.0) dot = 1.0;
      if (dot <-1.0) dot =-1.0;
      const Number &norpi = sqrt(pi*pi);
      const Number &norqi = sqrt(qi*qi);
      if (norpi*norqi<1e-5) continue;
      const Number &angle = acos(dot/(norpi*norqi));
      Vector3 axis = cross_product(pi, qi);
      const Number &noraxis = sqrt(axis*axis);
      if (noraxis<1e-5) continue;
      axis = axis/noraxis;
      vri[j] = axis*angle;
      //~ if (isnan(vri[j][0])) vri[j] = Vector3(0,0,0);
   }
   
   Number swi = 0.0;
   Vector3 swri(0,0,0);
   for (unsigned int j=0; j<NCTRLPOINTS; j++) {
      float W = weight(p,P[j]);
      pjhat[0] = P[j][0]-pstar[0]; pjhat[1] = P[j][1]-pstar[1]; pjhat[2] = P[j][2]-pstar[2];
      Vector3 pi(pjhat[0], pjhat[1], pjhat[2]);
      const Number &norpi = sqrt(pi*pi);
      if (norpi<1e-5) continue;
      swi  = swi + W*norpi;
      swri = swri + vri[j]*W*norpi;
   }
   Vector3 r = swri/swi;
   
   Number angle = sqrt(r*r);
   //~ assert(angle > 0);
   r = r/angle;
   const Number &x=r[0], &y=r[1], &z=r[2];
   Number c = cos(angle);
   Number s = sin(angle);
   Number t = 1.0f - c;
   float R[3][3];
   R[0][0]=t*x*x+c;   R[0][1]=t*x*y-z*s; R[0][2]=t*x*z+y*s;
   R[1][0]=t*x*y+z*s; R[1][1]=t*y*y+c;   R[1][2]=t*y*z-x*s;
   R[2][0]=t*x*z-y*s; R[2][1]=t*y*z+x*s; R[2][2]=t*z*z+c;
   
   float vhat[3] = {p[0]-pstar[0], p[1]-pstar[1], p[2]-pstar[2]};
   pd[0] = qstar[0] + R[0][0]*vhat[0] + R[0][1]*vhat[1] + R[0][2]*vhat[2];
   pd[1] = qstar[1] + R[1][0]*vhat[0] + R[1][1]*vhat[1] + R[1][2]*vhat[2];
   pd[2] = qstar[2] + R[2][0]*vhat[0] + R[2][1]*vhat[1] + R[2][2]*vhat[2];
}

LMatrix buildQ(const Number v[]);
LMatrix buildW(const Number v[]);
LMatrix buildK(const Number v[]);

/// A solution involving unitary quaternions
void mlsEval_dualquat(const vector<Point3> &P, const vector<Point3> &Q, float p[], float pd[]) {
   
   LMatrix C1(4,4), C3(4,4); // 4x4 temporal matrices used for computing R
   C1.zero(); C3.zero();
   const unsigned int &N = P.size();
   for (unsigned int j=0; j<N; ++j) {
      Number pp[] = {P[j][0]*0.5, P[j][1]*0.5, P[j][2]*0.5, 0.0};
      Number qq[] = {Q[j][0]*0.5, Q[j][1]*0.5, Q[j][2]*0.5, 0.0};
      float W = weight(p,P[j]);
      LMatrix mQ = buildQ(qq);
      LMatrix mW = buildW(pp);
      //~ cout<<"W "<<W<<" (i,j) "<<i<<" "<<j<<endl;
      //~ cout<<"Q "<<mQ<<endl;
      //~ cout<<"W "<<mW<<endl;
      C1 += W*mQ*mW;
      C3 += W*(mW-mQ);
      //~ cout<<"C1 "<<C1<<endl;
      //~ cout<<"C3 "<<C3<<endl;
   }
   C1 *= -2.0;
   C3 *= 2.0;
   LMatrix A = (1.0/Number(4.0*N))*t(C3)*C3 - C1;
   //~ LMatrix A = 0.5*(t(C3)*C3 - C1 - t(C1));
   
   //~ cout<<"A: "<<endl<<A<<endl;
   
   // compute the eigenvalues/eigenvectors
   vector<double> er, ei;
   vector<LVector> vr, vi;
   A.dgeev(er, ei, vr, vi);
   
   //~ cout<<"ok "<<er[0]<<" "<<er[1]<<" "<<er[2]<<" "<<er[3]<<endl;
   // get the eigenvector corresponding to the largest eignevalue
   int lmax = 0, rmax = 2, imax;
   if (er[0] < er[1]) lmax = 1;
   if (er[2] < er[3]) rmax = 3;
   (er[lmax] < er[rmax])? (imax=rmax):(imax=lmax);
   const LVector &q = vr[imax];
   
   //~ cout<<"q (imax)"<<imax<<" : "<<q(0)<<" "<<q(1)<<" "<<q(2)<<" "<<q(3)<<endl;
   //~ // build the rotation matrix
   const Number ro[]={q(0),q(1),q(2)};
   //~ buildK(ro);
   //~ VectorCPPL qv(3);
   //~ qv(0)=q(0); qv(1)=q(1); qv(2)=q(2);
   //~ R = (q(3)*q(3) - CPPL::t(qv)*qv)*mI + 2.0*qv*CPPL::t(qv) + 2.0*q(3)*mK;
   //~ cout<<"R dual quat: "<<endl<<R<<endl;
   
   //~ cout<<"Rot: "<<endl<<CPPL::t(R)*R<<endl;
   
   LMatrix R(3,3);
   const Number &q02 = q(0)*q(0); const Number &q12 = q(1)*q(1);
   const Number &q22 = q(2)*q(2); const Number &q32 = q(3)*q(3);
   R(0,0)=q02+q12-q22-q32;           R(0,1)=2.0*(q(1)*q(2)-q(0)*q(3)); R(0,2)=2.0*(q(1)*q(3)+q(0)*q(2)); 
   R(1,0)=2.0*(q(2)*q(1)+q(0)*q(3)); R(1,1)=q02-q12+q22-q32;           R(1,2)=2.0*(q(2)*q(3)-q(0)*q(1)); 
   R(2,0)=2.0*(q(3)*q(1)-q(0)*q(2)); R(2,1)=2.0*(q(3)*q(2)+q(0)*q(1)); R(2,2)=q02-q12-q22+q32; 
   
   
   // translation
   LMatrix mW = buildW(ro);
   LVector T = t(mW)*(-1.0)*C3*q;
   //cout<<"T: "<<T<<endl;
   
   LVector vhat(3);
   vhat(0) = p[0]; vhat(1) = p[1]; vhat(2) = p[2];
   const LVector mv = R*vhat;
   pd[0] = mv(0);
   pd[1] = mv(1);
   pd[2] = mv(2);
}

LMatrix buildQ(const Number v[]) {
   
   LMatrix mI(3,3);
   mI.identity();
   LMatrix mQ(4,4);
   LMatrix mK = buildK(v);
   mK = v[3]*mI + mK;
   mQ(0,0) = mK(0,0); mQ(0,1) = mK(0,1); mQ(0,2) = mK(0,2); mQ(0,3) = v[0];
   mQ(1,0) = mK(1,0); mQ(1,1) = mK(1,1); mQ(1,2) = mK(1,2); mQ(1,3) = v[1];
   mQ(2,0) = mK(2,0); mQ(2,1) = mK(2,1); mQ(2,2) = mK(2,2); mQ(2,3) = v[2];
   mQ(3,0) =-v[0];    mQ(3,1) =-v[1];    mQ(3,2) =-v[2];    mQ(3,3) = v[3];
   return mQ;
}

LMatrix buildW(const Number v[]) {
   
   LMatrix mI(3,3);
   mI.identity();
   LMatrix mW(4,4);
   LMatrix mK = buildK(v);
   mK = v[3]*mI - mK;
   mW(0,0) = mK(0,0); mW(0,1) = mK(0,1); mW(0,2) = mK(0,2); mW(0,3) = v[0];
   mW(1,0) = mK(1,0); mW(1,1) = mK(1,1); mW(1,2) = mK(1,2); mW(1,3) = v[1];
   mW(2,0) = mK(2,0); mW(2,1) = mK(2,1); mW(2,2) = mK(2,2); mW(2,3) = v[2];
   mW(3,0) =-v[0];    mW(3,1) =-v[1];    mW(3,2) =-v[2];    mW(3,3) = v[3];
   return mW;
}

LMatrix buildK(const Number v[]) {
   
   LMatrix mK(3,3);
   mK(0,0) = 0.0;  mK(0,1) =-v[2]; mK(0,2) = v[1];
   mK(1,0) = v[2]; mK(1,1) = 0.0;  mK(1,2) =-v[0];
   mK(2,0) =-v[1]; mK(2,1) = v[0]; mK(2,2) = 0.0;
   return mK;
}

#endif
